#include "Mouse_Drive.h"
/*********************************************************************************************************
  定义全局变量
*********************************************************************************************************/
MAZECOOR GmcMouse = {0, 0};                   /*  保存电脑鼠当前位置坐标      */
uint8 GucMouseDir = UP;                       /*  保存电脑鼠当前方向          */
uint8 GucMapBlock[MAZETYPE][MAZETYPE] = {0};  /*  GucMapBlock[x][y]           */
                                              /*  x,横坐标;y,纵坐标;          */
uint8 GucMapBlock0[MAZETYPE][MAZETYPE] = {0}; /*  中心算法         */
                                              /*  bit3~bit0分别代表左下右上   */
                                              /*  0:该方向无路，1:该方向有路  */
uint8 GucMapBlock1[MAZETYPE][MAZETYPE] = {0x0f};
uint8 GucMouseStart = 0; /* 电脑鼠启动        */
uint8 GucFrontJinju = 0; /* 前方红外近距在搜索时等高图制作中用   */
uint8 GucCrossroad = 0;  /* 十字路口数，冲刺时用，若十字路口多降低最高冲刺速度   */
static uint32 GW;        /*小车转动角度*/

static __MOTOR __GmLeft = {0, 0, 0, 0, 0, 0};  /*  定义并初始化左电机状态      */
static __MOTOR __GmRight = {0, 0, 0, 0, 0, 0}; /*  定义并初始化右电机状态      */
static __PID __GmLPID;                         /*  定义左电机PID      */
static __PID __GmRPID;                         /*  定义右电机PID     */
static __PID __GmSPID;                         /*  直线PID     */
static __PID __GmWPID;                         /*  旋转PID     */
static uint8 __GucMouseState = __STOP;         /*  保存电脑鼠当前运行状态      */
static int32 __GiMaxSpeed = SEARCHSPEED;       /*  保存允许运行的最大速度      */
static uint8 __GucDistance[5] = {0};           /*  记录传感器状态              */
uint16 GusFreq_F = 36200;                      /*  前方红外频率              */
uint16 GusFreq_FJ = 19200;                     /*  前方近距红外频率              */
uint16 GusFreq_X = 30000;                      /*  斜45度红外频率              */
uint16 GusFreq_LF = 31700;                     /*  左右红外远距频率              */
uint16 GusFreq_L = 18300;                      /*  左右红外近距频率              */
static int16 GsTpusle_T = 0;                   /*  左轮校正减少的速度值              */
static uint8 GuiSpeedCtr = 0;
static uint16 GuiTpusle_LR = 0;
static uint16 GuiTpusle_S = 0;
static uint8 GucFrontNear = 0;
uint8 GucGoHead = 0;
uint8 GucFangXiang = 0;
uint8 GucDirTemp = 0;
uint16 DIS[10] = {0};
uint8 Tab = 4; //红外调试选项

uint16 count = 0;

extern u16 voltageDetectRef;
extern __IO uint16_t ADC_ConvertedValue[5];
extern float ADC_ConvertedValueLocal[4];
float W;
uint16 Sunlight[4];
uint16 ucIRCheck[4];

//红外的设定值，最后改成数组形式
uint16 GusDistance_L_Near = 461;  //左红外近
uint16 GusDistance_L_Mid = 384;   //左红外中
uint16 GusDistance_L_Far = 110;   //左红外远
uint16 GusDistance_R_Near = 409;  //右红外近
uint16 GusDistance_R_Mid = 329;   //右红外中
uint16 GusDistance_R_Far = 100;   //右红外近
uint16 GusDistance_FL_Near = 200; //近距，用来判断停止
uint16 GusDistance_FR_Near = 200;
uint16 GusDistance_FL_Far = 110; //远距用来判断墙壁
uint16 GusDistance_FR_Far = 200;
u8 flag = 0;
u8 ir_flag = 0;
u8 mm = 0;
u8 backflag = 0;
u8 backflag2 = 0;
u16 time;
/*********************************************************************************************************
** Function name:       __delay
** Descriptions:        延时函数
** input parameters:    uiD :延时参数，值越大，延时越久
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void __delay(uint32 uiD)
{
    for (; uiD; uiD--)
        ;
}

void Delay_us(u32 Nus)
{
    SysTick->LOAD = Nus * 9;
    SysTick->CTRL |= 0x01;
    while (!(SysTick->CTRL & (1 << 16)))
        ;
    SysTick->CTRL = 0X00000000;
    SysTick->VAL = 0X00000000;
}
/*********************************************************************************************************
** Function name:       PIDInit
** Descriptions:        PID初始化
** input parameters:    无
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void PIDInit(void)
{
    __GmLPID.usEncoder_new = 32768;
    __GmLPID.usFeedBack = 0; //速度反馈值
    __GmLPID.sFeedBack = 0;

    __GmRPID.usEncoder_new = 32768;
    __GmRPID.usFeedBack = 0; //速度反馈值
    __GmRPID.sFeedBack = 0;

    __GmSPID.sRef = 0; //速度设定值
    __GmSPID.sFeedBack = 0;
    __GmSPID.sPreError = 0;  //前一次，速度误差,,vi_Ref - vi_FeedBack
    __GmSPID.sPreDerror = 0; //前一次，速度误差之差，d_error-PreDerror;

    __GmSPID.fKp = __KP;
    __GmSPID.fKi = __KI;
    __GmSPID.fKd = __KD;

    __GmSPID.iPreU = 0; //电机控制输出值

    __GmWPID.sRef = 0; //速度设定值
    __GmWPID.sFeedBack = 0;
    __GmWPID.sPreError = 0;  //前一次，速度误差,,vi_Ref - vi_FeedBack
    __GmWPID.sPreDerror = 0; //前一次，速度误差之差，d_error-PreDerror;

    __GmWPID.fKp = __KP;
    __GmWPID.fKi = __KI;
    __GmWPID.fKd = __KD;

    __GmWPID.iPreU = 0; //电机控制输出值
}

/*********************************************************************************************************
** Function name:       __SPIDContr
** Descriptions:        直线PID控制
** input parameters:    无
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void __SPIDContr(void)
{
    float error, d_error, dd_error;
    static uint8 K_I = 1;
    error = __GmSPID.sRef - __GmSPID.sFeedBack; // 偏差计算
    d_error = error - __GmSPID.sPreError;
    dd_error = d_error - __GmSPID.sPreDerror;
    if (error > Deadband)
        error -= Deadband;
    else if (error < -Deadband)
        error += Deadband;
    else
        error = 0;
    if ((error > error_IMAX) || (error < -error_IMAX))
        K_I = 0;
    else
        K_I = 1;

    __GmSPID.sPreError = error; //存储当前偏差
    __GmSPID.sPreDerror = d_error;

    __GmSPID.iPreU += (int16)(__GmSPID.fKp * d_error + K_I * __GmSPID.fKi * error + __GmSPID.fKd * dd_error);
}
/*********************************************************************************************************
** Function name:       __WPIDContr
** Descriptions:        旋转方向PID控制
** input parameters:    无
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void __WPIDContr(void)
{
    float error, d_error, dd_error;
    static uint8 K_I = 1;
    error = __GmWPID.sRef + GsTpusle_T - __GmWPID.sFeedBack; // 偏差计算
    d_error = error - __GmWPID.sPreError;
    dd_error = d_error - __GmWPID.sPreDerror;
    if (error > Deadband)
        error -= Deadband;
    else if (error < -Deadband)
        error += Deadband;
    else
        error = 0;
    if ((error > error_IMAX) || (error < -error_IMAX))
        K_I = 0;
    else
        K_I = 1;

    __GmWPID.sPreError = error; //存储当前偏差
    __GmWPID.sPreDerror = d_error;
    __GmWPID.iPreU += (int16)(__GmWPID.fKp * d_error + K_I * __GmWPID.fKi * error + __GmWPID.fKd * dd_error);
}

void voltageDetect(void)
{
    u16 w, sum = 0, i;
    for (i = 0; i < 5; i++)
    {
        if (ADC_ConvertedValue[4] >= voltageDetectRef)
        {
            w = ADC_ConvertedValue[4] - voltageDetectRef;
        }
        else
        {
            w = voltageDetectRef - ADC_ConvertedValue[4];
        }
        sum = sum + w;
    }
    w = sum / 5;
    GW = GW + w;
}

/*********************************************************************************************************
** Function name:      __PIDContr
** Descriptions:        PID控制，通过脉冲数控制电机
** input parameters:    无
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void __PIDContr(void)
{
    __SPIDContr();
    __WPIDContr();
    __GmLeft.sSpeed = __GmSPID.iPreU - __GmWPID.iPreU;
    if (__GmLeft.sSpeed >= 0)
    {
        __GmLeft.cDir = __MOTORGOAHEAD;
        if (__GmLeft.sSpeed >= U_MAX) //速度PID，防止调节最高溢出
            __GmLeft.sSpeed = U_MAX;
        if (__GmLeft.sSpeed <= U_MIN) //速度PID，防止调节最低溢出
            __GmLeft.sSpeed = U_MIN;
    }
    else
    {
        __GmLeft.cDir = __MOTORGOBACK;
        __GmLeft.sSpeed *= -1;
        if (__GmLeft.sSpeed >= U_MAX) //速度PID，防止调节最高溢出
            __GmLeft.sSpeed = U_MAX;
        if (__GmLeft.sSpeed <= U_MIN) //速度PID，防止调节最低溢出
            __GmLeft.sSpeed = U_MIN;
    }

    __GmRight.sSpeed = __GmSPID.iPreU + __GmWPID.iPreU;
    if (__GmRight.sSpeed >= 0)
    {
        __GmRight.cDir = __MOTORGOAHEAD;
        if (__GmRight.sSpeed >= U_MAX) //速度PID，防止调节最高溢出
            __GmRight.sSpeed = U_MAX;
        if (__GmRight.sSpeed <= U_MIN) //速度PID，防止调节最低溢出
            __GmRight.sSpeed = U_MIN;
    }
    else
    {
        __GmRight.cDir = __MOTORGOBACK;
        __GmRight.sSpeed *= -1;
        if (__GmRight.sSpeed >= U_MAX) //速度PID，防止调节最高溢出
            __GmRight.sSpeed = U_MAX;
        if (__GmRight.sSpeed <= U_MIN) //速度PID，防止调节最低溢出
            __GmRight.sSpeed = U_MIN;
    }
    __rightMotorContr();
    __leftMotorContr();
}
/*********************************************************************************************************
** Function name:       __Encoder
** Descriptions:        采集编码器输出的脉冲
** input parameters:    无
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void __Encoder(void)
{
    static u16 Dir_L;
    static u16 Dir_R;

    __GmLPID.usEncoder_new = TIM_GetCounter(TIM2);
    __GmRPID.usEncoder_new = TIM_GetCounter(TIM3);

    Dir_R = TIM3->CR1;
    Dir_R = (Dir_R & 0x0010) >> 4;

    Dir_L = TIM2->CR1;
    Dir_L = (Dir_L & 0x0010) >> 4;

    if (Dir_L == 1) //向下计数  向后退
    {
        __GmLPID.usFeedBack = 32768 - __GmLPID.usEncoder_new;
        TIM_SetCounter(TIM2, 32768);
        __GmLeft.uiPulseCtr += __GmLPID.usFeedBack;
        __GmLeft.cRealDir = __MOTORGOBACK;
        __GmLPID.sFeedBack = -1 * __GmLPID.usFeedBack;
    }
    else
    {
        __GmLPID.usFeedBack = __GmLPID.usEncoder_new - 32768;
        TIM_SetCounter(TIM2, 32768);
        __GmLeft.uiPulseCtr += __GmLPID.usFeedBack;
        __GmLeft.cRealDir = __MOTORGOAHEAD;
        __GmLPID.sFeedBack = __GmLPID.usFeedBack;
    }

    if (Dir_R == 1) //向下计数  前进
    {
        __GmRPID.usFeedBack = 32768 - __GmRPID.usEncoder_new;
        TIM_SetCounter(TIM3, 32768);
        __GmRight.uiPulseCtr += __GmRPID.usFeedBack;
        __GmRight.cRealDir = __MOTORGOAHEAD;
        __GmRPID.sFeedBack = __GmRPID.usFeedBack;
    }
    else
    {
        __GmRPID.usFeedBack = __GmRPID.usEncoder_new - 32768;
        TIM_SetCounter(TIM3, 32768);
        __GmRight.uiPulseCtr += __GmRPID.usFeedBack;
        __GmRight.cRealDir = __MOTORGOBACK;
        __GmRPID.sFeedBack = -1 * __GmRPID.usFeedBack;
    }

    __GmSPID.sFeedBack = (__GmRPID.sFeedBack + __GmLPID.sFeedBack) / 2;
    __GmWPID.sFeedBack = (__GmRPID.sFeedBack - __GmLPID.sFeedBack) / 2;
}
void __irSendFreq(int8 __cNumber)
{
    switch (__cNumber)
    {
    case 1:                              /*前方红外*/
        GPIO_SetBits(GPIOA, GPIO_Pin_5); //前方左 开
        break;

    case 2:                               /*左右红外*/
        GPIO_SetBits(GPIOC, GPIO_Pin_13); //前方右 开
        break;
    case 3:
        GPIO_SetBits(GPIOA, GPIO_Pin_3); //左 开
        break;
    case 4:
        GPIO_SetBits(GPIOC, GPIO_Pin_2); //右 开
        break;
    default:
        break;
    }
}

void __irCheck(void)
{
    static uint8 ucState = 0;
    switch (ucState)
    {
    case 0:
        ucIRCheck[3] = ADC_ConvertedValue[3] - Sunlight[3]; //
        if ((ucIRCheck[3] < 0) || (ucIRCheck[3] > 30000))
            ucIRCheck[3] = 0;

        GPIO_ResetBits(GPIOC, GPIO_Pin_2);
        if (ucIRCheck[3] > GusDistance_R_Far)
        {
            __GucDistance[__RIGHT] |= 0x01;
        }
        else
        {
            __GucDistance[__RIGHT] &= 0xfe;
        }

        if (ucIRCheck[3] > GusDistance_R_Mid)
        {
            __GucDistance[__RIGHT] |= 0x02;
        }

        else
        {
            __GucDistance[__RIGHT] &= 0xfd;
        }

        if (ucIRCheck[3] > GusDistance_R_Near)
        {
            __GucDistance[__RIGHT] |= 0x04;
        }

        else
        {
            __GucDistance[__RIGHT] &= 0xfb;
        }
        Sunlight[0] = 0;
        delay(1000);
        Sunlight[0] = ADC_ConvertedValue[0];
        __irSendFreq(1);
        break;

    case 1:
        ucIRCheck[0] = ADC_ConvertedValue[0] - Sunlight[0]; //
        if ((ucIRCheck[0] < 0) || (ucIRCheck[0] > 30000))
            ucIRCheck[0] = 0;

        GPIO_ResetBits(GPIOA, GPIO_Pin_5);

        if (ucIRCheck[0] > GusDistance_FL_Far)
        {
            __GucDistance[__FRONTL] |= 0x01;
        }
        else
        {
            __GucDistance[__FRONTL] &= 0xfe;
        }
        if (ucIRCheck[0] > GusDistance_FL_Near)
        {
            __GucDistance[__FRONTL] |= 0x02;
        }
        else
        {
            __GucDistance[__FRONTL] &= 0xfd;
        }
        Sunlight[1] = 0;
        delay(1000);
        Sunlight[1] = ADC_ConvertedValue[1];
        __irSendFreq(2);
        break;

    case 2:
        ucIRCheck[1] = ADC_ConvertedValue[1] - Sunlight[1]; //
        if ((ucIRCheck[1] < 0) || (ucIRCheck[1] > 30000))
            ucIRCheck[1] = 0;
        GPIO_ResetBits(GPIOC, GPIO_Pin_13);
        if (ucIRCheck[1] > GusDistance_FR_Far)
        {
            __GucDistance[__FRONTR] |= 0x01;
        }
        else
        {
            __GucDistance[__FRONTR] &= 0xfe;
        }

        if (ucIRCheck[1] > GusDistance_FL_Near)
        {
            __GucDistance[__FRONTR] |= 0x02;
        }
        else
        {
            __GucDistance[__FRONTR] &= 0xfd;
        }
        if ((ucIRCheck[0] > GusDistance_FL_Far) && (ucIRCheck[1] > GusDistance_FR_Far))
            GucGoHead = 1;
        else
            GucGoHead = 0;
        Sunlight[2] = 0;
        delay(1000);
        Sunlight[2] = ADC_ConvertedValue[2];
        __irSendFreq(3);
        break;

    case 3:
        ucIRCheck[2] = ADC_ConvertedValue[2] - Sunlight[2]; //
        if ((ucIRCheck[2] < 0) || (ucIRCheck[2] > 30000))
            ucIRCheck[2] = 0;
        GPIO_ResetBits(GPIOA, GPIO_Pin_3);
        if (ucIRCheck[2] > GusDistance_L_Far)
        {
            __GucDistance[__LEFT] |= 0x01;
        }
        else
        {
            __GucDistance[__LEFT] &= 0xfe;
        }

        if (ucIRCheck[2] > GusDistance_L_Mid)
        {
            __GucDistance[__LEFT] |= 0x02;
        }
        else
        {
            __GucDistance[__LEFT] &= 0xfd;
        }

        if (ucIRCheck[2] > GusDistance_L_Near)
        {
            __GucDistance[__LEFT] |= 0x04;
        }
        else
        {
            __GucDistance[__LEFT] &= 0xfb;
        }
        Sunlight[3] = 0;
        delay(1000);
        Sunlight[3] = ADC_ConvertedValue[3];
        __irSendFreq(4);
        break;
    default:
        break;
    }
    ucState = (ucState + 1) % 4;
}

void sensorDebug(void)
{
    zlg7289Download(2, 0, 0, __GucDistance[__LEFT]);   //传感器左
    zlg7289Download(2, 1, 0, __GucDistance[__FRONTL]); //传感器前左
    zlg7289Download(2, 2, 0, __GucDistance[__FRONTR]); //传感器前右
    zlg7289Download(2, 3, 0, __GucDistance[__RIGHT]);  //传感器右
}

/*********************************************************************************************************
** Function name:       __rightMotorContr
** Descriptions:        右直流电机驱动
** input parameters:    无
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void __rightMotorContr(void)
{
    switch (__GmRight.cDir)
    {
    case __MOTORGOAHEAD:
        TIM_SetCompare1(TIM1, __GmRight.sSpeed);
        TIM_SetCompare2(TIM1, 0);
        break;

    case __MOTORGOBACK:
        TIM_SetCompare1(TIM1, 0);
        TIM_SetCompare2(TIM1, __GmRight.sSpeed);
        break;

    case __MOTORGOSTOP:
        TIM_SetCompare1(TIM1, 0);
        TIM_SetCompare2(TIM1, 0);
        break;

    default:
        break;
    }
}

/*********************************************************************************************************
** Function name:       __leftMotorContr
** Descriptions:        左直流电机驱动
** input parameters:    __GmLeft.cDir :电机运行方向
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void __leftMotorContr(void)
{
    switch (__GmLeft.cDir)
    {
    case __MOTORGOAHEAD:
        TIM_SetCompare4(TIM1, 0);
        TIM_SetCompare3(TIM1, __GmLeft.sSpeed);
        break;

    case __MOTORGOBACK:
        TIM_SetCompare4(TIM1, __GmLeft.sSpeed);
        TIM_SetCompare3(TIM1, 0);
        break;

    case __MOTORGOSTOP:
        TIM_SetCompare3(TIM1, 0);
        TIM_SetCompare4(TIM1, 0);
        break;

    default:
        break;
    }
}

/*********************************************************************************************************
** Function name:       __SpeedUp
** Descriptions:        电脑鼠加速程序
** input parameters:    无
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void __SpeedUp(void)
{
    uint16 Speed;
    Speed = __GmSPID.sFeedBack;
    if (__GmSPID.sRef < __GiMaxSpeed)
    {
        if (Speed >= __GmSPID.sRef)
        {
            __GmSPID.sRef = __GmSPID.sRef + 10;
        }
    }
}
/*********************************************************************************************************
** Function name:       __SpeedDown
** Descriptions:        电脑鼠减速程序
** input parameters:    无
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void __SpeedDown(void)
{
    uint16 Speed;
    Speed = __GmSPID.sFeedBack;
    if (__GmSPID.sRef >= MINSPEED)
    {
        if (Speed <= __GmSPID.sRef + 3)
        {
            __GmSPID.sRef = __GmSPID.sRef - 3;
        }
    }
}

/*********************************************************************************************************
** Function name:       startCheck
** Descriptions:        读取按键
** input parameters:    无
** output parameters:   无
** Returned value:      true:  按键已按下
**                      false: 按键未按下
*********************************************************************************************************/
uint8 startCheck(void)
{
    if (GPIO_ReadInputDataBit(GPIOC, __START) == 0)
    {
        __delay(50);
        while (GPIO_ReadInputDataBit(GPIOC, __START) == 0)
            ;
        return (true);
    }
    else
    {
        return (false);
    }
}

void TIM2_IROHandler(void)
{
    TIM_ClearFlag(TIM2, TIM_FLAG_Update);
}

void TIM3_IRQHandler(void)
{
    TIM_ClearFlag(TIM3, TIM_FLAG_Update);
}

/*********************************************************************************************************
** Function name:       SysTick_Handler
** Descriptions:        定时中断扫描。
** input parameters:    无
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void SysTick_Handler(void)
{
    static int8 n = 0, m = 0, k = 0, l = 0, a = 0, b = 0, c = 0, t = 0, w = 0;
    uint16 Sp;
    __Encoder();
    __irCheck();
    Sp = __GmSPID.sFeedBack;

    switch (__GmRight.cState)
    {
    case __MOTORSTOP: /*  停止，同时清零速度和脉冲值  */
        __GmRight.uiPulse = 0;
        __GmRight.uiPulseCtr = 0;
        __GmLeft.uiPulse = 0;
        __GmLeft.uiPulseCtr = 0;
        break;

    case __WAITONESTEP: /*  暂停一步                    */
        __GmRight.cState = __MOTORRUN;
        if ((((ucIRCheck[2] > GusDistance_L_Near) && (ucIRCheck[3] < GusDistance_R_Near)) && (ucIRCheck[3] > GusDistance_R_Mid))) //偏左
        {
            GsTpusle_T = -4;
        }
        else if ((((ucIRCheck[2] < GusDistance_L_Near) && (ucIRCheck[3] > GusDistance_R_Near)) && (ucIRCheck[2] > GusDistance_L_Mid))) //偏右
        {
            GsTpusle_T = 4;
        }
        if ((ucIRCheck[2] > GusDistance_L_Near) && (ucIRCheck[3] < GusDistance_R_Mid)) //偏左
        {
            GsTpusle_T = -6;
        }
        else if ((ucIRCheck[3] > GusDistance_R_Near) && (ucIRCheck[2] < GusDistance_L_Mid)) //偏右
        {
            GsTpusle_T = 6;
        }
        else if ((ucIRCheck[2] < GusDistance_L_Far) && ((ucIRCheck[3] > GusDistance_R_Far) && (ucIRCheck[3] < GusDistance_R_Mid)))
        {
            GsTpusle_T = -5;
        }

        else if ((ucIRCheck[3] < GusDistance_R_Far) && ((ucIRCheck[2] > GusDistance_L_Far) && (ucIRCheck[2] < GusDistance_L_Mid)))
        {
            GsTpusle_T = 5;
        }

        __PIDContr();
        break;

    case __MOTORRUN:                      /*  电机运行                    */
        if (__GucMouseState == __GOAHEAD) /*  根据传感器状态微调电机位置  */
        {
            if ((ucIRCheck[2] > GusDistance_L_Near) && (ucIRCheck[3] < GusDistance_R_Near))
            {
                if (n == 1)
                {
                    __GmRight.cState = __WAITONESTEP;
                }
                n++;
                n %= 2;
            }
            else if ((ucIRCheck[2] < GusDistance_L_Near) && (ucIRCheck[3] > GusDistance_R_Near))
            {
                if (a == 1)
                {
                    __GmRight.cState = __WAITONESTEP;
                }
                a++;
                a %= 2;
            }

            else if ((ucIRCheck[2] < GusDistance_L_Mid) && (ucIRCheck[2] > GusDistance_L_Far))
            {
                if (b == 1)
                {
                    __GmRight.cState = __WAITONESTEP;
                }
                b++;
                b %= 2;
            }
            else if ((ucIRCheck[3] < GusDistance_R_Mid) && (ucIRCheck[3] > GusDistance_R_Far))
            {
                if (c == 1)
                {
                    __GmRight.cState = __WAITONESTEP;
                }
                c++;
                c %= 2;
            }
            else
            {
                m = 0;
                n = 0;
                GsTpusle_T = 0;
            }

            if (GuiSpeedCtr == __SPEEDUP)
            {
                k = (k + 1) % 5; //20
                if (k == 4)
                    __SpeedUp();
            }
            else if (GuiSpeedCtr == __SPEEDDOWN)
            {
                k = (k + 1) % 10;
                if (k == 9)
                    __SpeedDown();
            }
            else
                ;
        }

        else
        {
            GsTpusle_T = 0;
            voltageDetect();
        }
        __PIDContr();
        break;

    case 4:
        GsTpusle_T = 0;
        __PIDContr();
        break;

    case 5:
        __GmRight.sSpeed = 10;
        __rightMotorContr();
        __GmRight.cState = __MOTORSTOP;
        __GmLeft.sSpeed = 10;
        __leftMotorContr();
        __GmLeft.cState = __MOTORSTOP;
        break;

    default:
        break;
    }
}

/*********************************************************************************************************
** Function name:       mazeSearch
** Descriptions:        前进N格
** input parameters:    iNblock: 前进的格数
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void mouseGoahead1(int8 cNBlock) //冲刺看前方红外，无坐标更新,当goto end速度大于20时设定速度为20
{
    int8 cL = 0, cR = 0, cCoor = 1, cB;
    GuiSpeedCtr = __SPEEDUP;
    if (__GmLeft.cState)
    {
        cCoor = 0;
    }
    if (cNBlock == 1)
    {
        cL = 1;
        cR = 1;
        if (GucFangXiang == GucDirTemp)
        {
            GuiTpusle_LR = 3200;
            GuiTpusle_S = 0;
        }
        else
        {
            GuiTpusle_LR = 3200;
            GuiTpusle_S = 0;
        }
        __GiMaxSpeed = 100;
    }
    else
    {
        GuiTpusle_LR = 0;
        GuiTpusle_S = 0;
    }
    GucFangXiang = GucDirTemp;
    if (((GmcMouse.cX == 7) && (GmcMouse.cY == 7)) ||
        ((GmcMouse.cX == 8) && (GmcMouse.cY == 8)) ||
        ((GmcMouse.cX == 7) && (GmcMouse.cY == 8)) ||
        ((GmcMouse.cX == 8) && (GmcMouse.cY == 7)))
    {
        cL = 0;
        cR = 0;
        GuiTpusle_LR = 0;
        GuiTpusle_S = 0;
    }
    if ((__GucMouseState == __TURNRIGHT) || (__GucMouseState == __TURNLEFT))
    {
        __GmLeft.uiPulseCtr = 14400;
        __GmRight.uiPulseCtr = 14400;
    }

    cB = cNBlock;
    __GucMouseState = __GOAHEAD;
    __GmRight.uiPulse = __GmRight.uiPulse + cNBlock * ONEBLOCK;
    __GmLeft.uiPulse = __GmLeft.uiPulse + cNBlock * ONEBLOCK;
    __GmRight.cState = __MOTORRUN;
    __GmLeft.cState = __MOTORRUN;
    if (cNBlock > 5)
    {
        __GiMaxSpeed = 140;
    }
    else if (cNBlock == 5)
    {
        __GiMaxSpeed = 132;
    }
    else if (cNBlock == 4)
    {
        __GiMaxSpeed = 120;
    }
    else if (cNBlock == 3)
    {
        __GiMaxSpeed = 108;
    }
    else if (cNBlock == 2)
    {
        __GiMaxSpeed = 100;
    }
    else
        ;
    while (__GmLeft.cState != __MOTORSTOP)
    {

        if (__GmLeft.uiPulseCtr >= ONEBLOCK)
        { /*  判断是否走完一格            */
            __GmLeft.uiPulse -= ONEBLOCK;
            __GmLeft.uiPulseCtr -= ONEBLOCK;
            if (cCoor)
            {
                cNBlock--;
                if (cNBlock == 0)
                    goto End;
                if (cNBlock < cB - 1) //给回速一个时间
                    GuiSpeedCtr = __SPEEDUP;
            }
            else
            {
                cCoor = 1;
            }
        }

        if (__GmRight.uiPulseCtr >= ONEBLOCK)
        { /*  判断是否走完一格            */
            __GmRight.uiPulse -= ONEBLOCK;
            __GmRight.uiPulseCtr -= ONEBLOCK;
        }
        if (cNBlock < 2)
        {
            if (__GmSPID.sFeedBack > 100)
            {
                GuiSpeedCtr = 3;
                __GmSPID.sRef = 100;
            }
            if (cL)
            {                                         /*  是否允许检测左边            */
                if (ucIRCheck[2] < GusDistance_L_Far) /*  左边有支路，跳出程序        */
                {
                    __GmRight.uiPulse = __GmRight.uiPulseCtr + 10000 - GuiTpusle_LR;
                    __GmLeft.uiPulse = __GmLeft.uiPulseCtr + 10000 - GuiTpusle_LR;
                    while (ucIRCheck[2] < GusDistance_L_Far)
                    {

                        if ((__GmLeft.uiPulseCtr + 600) > __GmLeft.uiPulse)
                        {
                            goto End;
                        }
                    }
                }
            }
            else
            { /*  左边有墙时开始允许检测左边  */
                if (ucIRCheck[2] > GusDistance_L_Far)
                {
                    cL = 1;
                }
            }
            if (cR)
            {                                         /*  是否允许检测右边            */
                if (ucIRCheck[3] < GusDistance_R_Far) /*  右边有支路，跳出程序        */
                {
                    __GmRight.uiPulse = __GmRight.uiPulseCtr + 30000 - GuiTpusle_LR;
                    __GmLeft.uiPulse = __GmLeft.uiPulseCtr + 30000 - GuiTpusle_LR;
                    while (ucIRCheck[3] < GusDistance_R_Far)
                    {

                        if ((__GmLeft.uiPulseCtr + 600) > __GmLeft.uiPulse)
                        {
                            goto End;
                        }
                    }
                }
            }
            else
            {
                if (ucIRCheck[3] > GusDistance_R_Far)
                { /*  右边有墙时开始允许检测右边  */
                    cR = 1;
                }
            }
        }
    }
    /*
     *  设定运行任务，让电脑鼠走到支路的中心位置
     */
End:; //GuiSpeedCtr= 3;
      //__GmSPID.sRef=18;
}
/*********************************************************************************************************
** Function name:      mouseGoaheadhui
** Descriptions:        前进N格,最后回起点用，要用前方红外
** input parameters:    iNblock: 前进的格数
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void mouseGoaheadhui(int8 cNBlock)
{
    int8 cL = 0, cR = 0, cCoor = 1, cB;
    //GusFreq_LF = 35400;
    GuiSpeedCtr = __SPEEDUP;
    if (__GmLeft.cState)
    {
        cCoor = 0;
    }
    if (cNBlock == 1)
    {
        cL = 1;
        cR = 1;
        //GuiTpusle_LR = 0;    //100
        if (GucFangXiang == GucDirTemp)
        {
            GuiTpusle_LR = 3200;
            GuiTpusle_S = 0;
        }
        else
        {
            GuiTpusle_LR = 3200;
            GuiTpusle_S = 0;
        }
        __GiMaxSpeed = 80;
    }
    else
    {
        GuiTpusle_LR = 0;
        GuiTpusle_S = 0;
        __GiMaxSpeed = 108;
    }
    GucFangXiang = GucDirTemp;
    if (((GmcMouse.cX == 7) && (GmcMouse.cY == 7)) ||
        ((GmcMouse.cX == 8) && (GmcMouse.cY == 8)) ||
        ((GmcMouse.cX == 7) && (GmcMouse.cY == 8)) ||
        ((GmcMouse.cX == 8) && (GmcMouse.cY == 7)))
    {
        cL = 0;
        cR = 0;
        GuiTpusle_LR = 0;
        GuiTpusle_S = 0;
    }
    if ((__GucMouseState == __TURNRIGHT) || (__GucMouseState == __TURNLEFT))
    {
        __GmLeft.uiPulseCtr = 14400;
        __GmRight.uiPulseCtr = 14400;
    }

    cB = cNBlock;
    __GucMouseState = __GOAHEAD;
    __GmRight.uiPulse = __GmRight.uiPulse + cNBlock * ONEBLOCK;
    __GmLeft.uiPulse = __GmLeft.uiPulse + cNBlock * ONEBLOCK;
    __GmRight.cState = __MOTORRUN;
    __GmLeft.cState = __MOTORRUN;

    while (__GmLeft.cState != __MOTORSTOP)
    {

        if (__GmLeft.uiPulseCtr >= ONEBLOCK)
        { /*  判断是否走完一格            */
            __GmLeft.uiPulse -= ONEBLOCK;
            __GmLeft.uiPulseCtr -= ONEBLOCK;
            if (cCoor)
            {
                cNBlock--;
                if (cNBlock == 0)
                    goto End;
                if (cNBlock < cB)
                    GuiSpeedCtr = __SPEEDUP;
            }
            else
            {
                cCoor = 1;
            }
        }

        if (__GmRight.uiPulseCtr >= ONEBLOCK)
        { /*  判断是否走完一格            */
            __GmRight.uiPulse -= ONEBLOCK;
            __GmRight.uiPulseCtr -= ONEBLOCK;
        }
        if (cNBlock < 2)
        {
            if (__GmSPID.sFeedBack > 72)
            {
                GuiSpeedCtr = 3;
                __GmSPID.sRef = 72;
            }
            if (cL)
            {                                         /*  是否允许检测左边            */
                if (ucIRCheck[2] < GusDistance_L_Far) /*  左边有支路，跳出程序        */
                {
                    __GmRight.uiPulse = __GmRight.uiPulseCtr + 3000 - GuiTpusle_LR; //3094(89mm)
                    __GmLeft.uiPulse = __GmLeft.uiPulseCtr + 3000 - GuiTpusle_LR;
                    while (ucIRCheck[2] < GusDistance_L_Far)
                    {

                        if ((__GmLeft.uiPulseCtr + 600) > __GmLeft.uiPulse)
                        {
                            goto End;
                        }
                    }
                }
            }
            else
            { /*  左边有墙时开始允许检测左边  */
                if (ucIRCheck[2] > GusDistance_L_Far)
                {
                    cL = 1;
                }
            }
            if (cR)
            {                                         /*  是否允许检测右边            */
                if (ucIRCheck[3] < GusDistance_R_Far) /*  右边有支路，跳出程序        */
                {
                    __GmRight.uiPulse = __GmRight.uiPulseCtr + 3000 - GuiTpusle_LR;
                    __GmLeft.uiPulse = __GmLeft.uiPulseCtr + 3000 - GuiTpusle_LR;
                    while (ucIRCheck[3] < GusDistance_R_Far)
                    {

                        if ((__GmLeft.uiPulseCtr + 600) > __GmLeft.uiPulse)
                        {
                            goto End;
                        }
                    }
                }
            }
            else
            {
                if (ucIRCheck[3] > GusDistance_R_Far)
                { /*  右边有墙时开始允许检测右边  */
                    cR = 1;
                }
            }
        }
    }
    /*
     *  设定运行任务，让电脑鼠走到支路的中心位置
     */
End:; //GuiSpeedCtr= 3;
      //__GmSPID.sRef=18;
}

void mouseGoahead(int8 cNBlock) //连续转弯用
{
    int8 cL = 0, cR = 0, cCoor = 1, cB;
    GuiSpeedCtr = __SPEEDUP;
    if (__GmLeft.cState)
    {
        cCoor = 0;
    }
    if (cNBlock == 1)
    {
        cL = 1;
        cR = 1;
        if (GucFangXiang == GucDirTemp)
        {
            GuiTpusle_LR = 13000;
            GuiTpusle_S = 0;
        }
        else
        {
            GuiTpusle_LR = 13000;
            GuiTpusle_S = 600;
        }
        __GiMaxSpeed = 120;
    }
    else
    {
        GuiTpusle_LR = 0;
        GuiTpusle_S = 0;
        __GiMaxSpeed = 120;
    }
    GucFangXiang = GucDirTemp;
    if (((GmcMouse.cX == 7) && (GmcMouse.cY == 7)) ||
        ((GmcMouse.cX == 8) && (GmcMouse.cY == 8)) ||
        ((GmcMouse.cX == 7) && (GmcMouse.cY == 8)) ||
        ((GmcMouse.cX == 8) && (GmcMouse.cY == 7)))
    {
        cL = 0;
        cR = 0;
        GuiTpusle_LR = 0;
        GuiTpusle_S = 0;
    }
    if ((__GucMouseState == __TURNRIGHT) || (__GucMouseState == __TURNLEFT))
    {
        __GmLeft.uiPulseCtr = 30000;
        __GmRight.uiPulseCtr = 30000;
    }

    cB = cNBlock;
    __GucMouseState = __GOAHEAD;
    __GmRight.uiPulse = __GmRight.uiPulse + cNBlock * ONEBLOCK;
    __GmLeft.uiPulse = __GmLeft.uiPulse + cNBlock * ONEBLOCK;
    __GmRight.cState = __MOTORRUN;
    __GmLeft.cState = __MOTORRUN;
    while (__GmLeft.cState != __MOTORSTOP)
    {

        if (__GmLeft.uiPulseCtr >= ONEBLOCK)
        { /*  判断是否走完一格            */
            __GmLeft.uiPulse -= ONEBLOCK;
            __GmLeft.uiPulseCtr -= ONEBLOCK;
            if (cCoor)
            {
                cNBlock--;
                if (cNBlock == 0)
                    goto End;
                if (cNBlock < cB - 1) //给回速一个时间
                    GuiSpeedCtr = __SPEEDUP;
            }
            else
            {
                cCoor = 1;
            }
        }

        if (__GmRight.uiPulseCtr >= ONEBLOCK)
        { /*  判断是否走完一格            */
            __GmRight.uiPulse -= ONEBLOCK;
            __GmRight.uiPulseCtr -= ONEBLOCK;
        }
        if (cNBlock < 2)
        {
            if (__GmSPID.sFeedBack > 120)
            {
                GuiSpeedCtr = 3;
                __GmSPID.sRef = 120;
            }
            if (cL)
            {                                         /*  是否允许检测左边            */
                if (ucIRCheck[2] < GusDistance_L_Far) /*  左边有支路，跳出程序        */
                {
                    __GmRight.uiPulse = __GmRight.uiPulseCtr + 21000 - GuiTpusle_LR; //3094(89mm)
                    __GmLeft.uiPulse = __GmLeft.uiPulseCtr + 21000 - GuiTpusle_LR;
                    while (ucIRCheck[2] < GusDistance_L_Far)
                    {

                        if ((__GmLeft.uiPulseCtr + 100) > __GmLeft.uiPulse)
                        {
                            goto End;
                        }
                    }
                }
            }
            else
            { /*  左边有墙时开始允许检测左边  */
                if (ucIRCheck[2] > GusDistance_L_Far)
                {
                    cL = 1;
                }
            }
            if (cR)
            {                                         /*  是否允许检测右边            */
                if (ucIRCheck[3] < GusDistance_R_Far) /*  右边有支路，跳出程序        */
                {
                    __GmRight.uiPulse = __GmRight.uiPulseCtr + 21000 - GuiTpusle_LR;
                    __GmLeft.uiPulse = __GmLeft.uiPulseCtr + 21000 - GuiTpusle_LR;
                    while (ucIRCheck[3] < GusDistance_R_Far)
                    {

                        if ((__GmLeft.uiPulseCtr + 100) > __GmLeft.uiPulse)
                        {
                            goto End;
                        }
                    }
                }
            }
            else
            {
                if (ucIRCheck[3] > GusDistance_R_Far)
                { /*  右边有墙时开始允许检测右边  */
                    cR = 1;
                }
            }
        }
    }
    /*
     *  设定运行任务，让电脑鼠走到支路的中心位置
     */
End:; //GuiSpeedCtr= 3;
      //__GmSPID.sRef=23;
}

void mouseGoahead_L(int8 cNBlock) //连续转弯用
{
    int8 cL = 0, cR = 0, cCoor = 1, cB;
    GuiSpeedCtr = __SPEEDUP;
    if (__GmLeft.cState)
    {
        cCoor = 0;
    }
    if (cNBlock == 1)
    {
        cL = 1;
        cR = 1;
        if (GucFangXiang == GucDirTemp)
        {
            GuiTpusle_LR = 13000;
            GuiTpusle_S = 0;
        }
        else
        {
            GuiTpusle_LR = 13000;
            GuiTpusle_S = 600;
        }
        __GiMaxSpeed = 120;
    }
    else
    {
        GuiTpusle_LR = 0;
        GuiTpusle_S = 0;
        __GiMaxSpeed = 120;
    }
    GucFangXiang = GucDirTemp;
    if (((GmcMouse.cX == 7) && (GmcMouse.cY == 7)) ||
        ((GmcMouse.cX == 8) && (GmcMouse.cY == 8)) ||
        ((GmcMouse.cX == 7) && (GmcMouse.cY == 8)) ||
        ((GmcMouse.cX == 8) && (GmcMouse.cY == 7)))
    {
        cL = 0;
        cR = 0;
        GuiTpusle_LR = 0;
        GuiTpusle_S = 0;
    }
    if ((__GucMouseState == __TURNRIGHT) || (__GucMouseState == __TURNLEFT))
    {
        __GmLeft.uiPulseCtr = 30000; //1182(34mm)
        __GmRight.uiPulseCtr = 30000;
    }

    cB = cNBlock;
    __GucMouseState = __GOAHEAD;
    __GmRight.uiPulse = __GmRight.uiPulse + cNBlock * ONEBLOCK;
    __GmLeft.uiPulse = __GmLeft.uiPulse + cNBlock * ONEBLOCK;
    __GmRight.cState = __MOTORRUN;
    __GmLeft.cState = __MOTORRUN;
    while (__GmLeft.cState != __MOTORSTOP)
    {

        if (__GmLeft.uiPulseCtr >= ONEBLOCK)
        { /*  判断是否走完一格            */
            __GmLeft.uiPulse -= ONEBLOCK;
            __GmLeft.uiPulseCtr -= ONEBLOCK;
            if (cCoor)
            {
                cNBlock--;
                if (cNBlock == 0)
                    goto End;
                if (cNBlock < cB - 1) //给回速一个时间
                    GuiSpeedCtr = __SPEEDUP;
            }
            else
            {
                cCoor = 1;
            }
        }

        if (__GmRight.uiPulseCtr >= ONEBLOCK)
        { /*  判断是否走完一格            */
            __GmRight.uiPulse -= ONEBLOCK;
            __GmRight.uiPulseCtr -= ONEBLOCK;
        }
        if (cNBlock < 2)
        {
            if (__GmSPID.sFeedBack > 120)
            {
                GuiSpeedCtr = 3;
                __GmSPID.sRef = 120;
            }
            if (cL)
            {                                         /*  是否允许检测左边            */
                if (ucIRCheck[2] < GusDistance_L_Far) /*  左边有支路，跳出程序        */
                {
                    __GmRight.uiPulse = __GmRight.uiPulseCtr + 21000 - GuiTpusle_LR; //3094(89mm)
                    __GmLeft.uiPulse = __GmLeft.uiPulseCtr + 21000 - GuiTpusle_LR;
                    while (ucIRCheck[2] < GusDistance_L_Far)
                    {

                        if ((__GmLeft.uiPulseCtr + 100) > __GmLeft.uiPulse)
                        {
                            goto End;
                        }
                    }
                }
            }
            else
            { /*  左边有墙时开始允许检测左边  */
                if (ucIRCheck[2] > GusDistance_L_Far)
                {
                    cL = 1;
                }
            }
            if (cR)
            {                                         /*  是否允许检测右边            */
                if (ucIRCheck[3] < GusDistance_R_Far) /*  右边有支路，跳出程序        */
                {
                    __GmRight.uiPulse = __GmRight.uiPulseCtr + 21000 - GuiTpusle_LR;
                    __GmLeft.uiPulse = __GmLeft.uiPulseCtr + 21000 - GuiTpusle_LR;
                    while (ucIRCheck[3] < GusDistance_R_Far)
                    {

                        if ((__GmLeft.uiPulseCtr + 100) > __GmLeft.uiPulse)
                        {
                            goto End;
                        }
                    }
                }
            }
            else
            {
                if (ucIRCheck[3] > GusDistance_R_Far)
                { /*  右边有墙时开始允许检测右边  */
                    cR = 1;
                }
            }
        }
    }
    /*
     *  设定运行任务，让电脑鼠走到支路的中心位置
     */
End:; //GuiSpeedCtr= 3;
      //__GmSPID.sRef=23;
}

void mouseGoahead_Llow(int8 cNBlock) //连续转弯用,如有十字路口低速、不加速
{
    int8 cL = 0, cR = 0, cCoor = 1, cB;
    GuiSpeedCtr = __SPEEDUP;
    if (__GmLeft.cState)
    {
        cCoor = 0;
    }
    if (cNBlock == 1)
    {
        cL = 1;
        cR = 1;
        if (GucFangXiang == GucDirTemp)
        {
            GuiTpusle_LR = 3200;
            GuiTpusle_S = 0;
        }
        else
        {
            GuiTpusle_LR = 3600;
            GuiTpusle_S = 600;
        }
        __GiMaxSpeed = 180;
    }
    else
    {
        GuiTpusle_LR = 0;
        GuiTpusle_S = 0;
        __GiMaxSpeed = 180;
    }
    GucFangXiang = GucDirTemp;
    if (((GmcMouse.cX == 7) && (GmcMouse.cY == 7)) ||
        ((GmcMouse.cX == 8) && (GmcMouse.cY == 8)) ||
        ((GmcMouse.cX == 7) && (GmcMouse.cY == 8)) ||
        ((GmcMouse.cX == 8) && (GmcMouse.cY == 7)))
    {
        cL = 0;
        cR = 0;
        GuiTpusle_LR = 0;
        GuiTpusle_S = 0;
    }
    if ((__GucMouseState == __TURNRIGHT) || (__GucMouseState == __TURNLEFT))
    {
        __GmLeft.uiPulseCtr = 14400; //1182(34mm)
        __GmRight.uiPulseCtr = 14400;
    }

    cB = cNBlock;
    __GucMouseState = __GOAHEAD;
    __GmRight.uiPulse = __GmRight.uiPulse + cNBlock * ONEBLOCK;
    __GmLeft.uiPulse = __GmLeft.uiPulse + cNBlock * ONEBLOCK;
    __GmRight.cState = __MOTORRUN;
    __GmLeft.cState = __MOTORRUN;

    while (__GmLeft.cState != __MOTORSTOP)
    {

        if (__GmLeft.uiPulseCtr >= ONEBLOCK)
        { /*  判断是否走完一格            */
            __GmLeft.uiPulse -= ONEBLOCK;
            __GmLeft.uiPulseCtr -= ONEBLOCK;
            if (cCoor)
            {
                cNBlock--;
                if (cNBlock == 0)
                    goto End;
                if (cNBlock < cB)
                    GuiSpeedCtr = __SPEEDUP;
            }
            else
            {
                cCoor = 1;
            }
        }

        if (__GmRight.uiPulseCtr >= ONEBLOCK)
        { /*  判断是否走完一格            */
            __GmRight.uiPulse -= ONEBLOCK;
            __GmRight.uiPulseCtr -= ONEBLOCK;
        }
        if (cNBlock < 2)
        {
            if (__GmSPID.sFeedBack > 96)
            {
                GuiSpeedCtr = 3;
                __GmSPID.sRef = 96;
            }
            if (cL)
            {                                         /*  是否允许检测左边            */
                if (ucIRCheck[2] < GusDistance_L_Far) /*  左边有支路，跳出程序        */
                {
                    __GmRight.uiPulse = __GmRight.uiPulseCtr + 5200 - GuiTpusle_LR; //3094(89mm)
                    __GmLeft.uiPulse = __GmLeft.uiPulseCtr + 5200 - GuiTpusle_LR;
                    while (ucIRCheck[2] < GusDistance_L_Far)
                    {

                        if ((__GmLeft.uiPulseCtr + 600) > __GmLeft.uiPulse)
                        {
                            goto End;
                        }
                    }
                }
            }
            else
            { /*  左边有墙时开始允许检测左边  */
                if (ucIRCheck[2] > GusDistance_L_Far)
                {
                    cL = 1;
                }
            }
            if (cR)
            {                                         /*  是否允许检测右边            */
                if (ucIRCheck[3] < GusDistance_R_Far) /*  右边有支路，跳出程序        */
                {
                    __GmRight.uiPulse = __GmRight.uiPulseCtr + 2600 - GuiTpusle_LR;
                    __GmLeft.uiPulse = __GmLeft.uiPulseCtr + 2600 - GuiTpusle_LR;
                    while (ucIRCheck[3] < GusDistance_R_Far)
                    {

                        if ((__GmLeft.uiPulseCtr + 600) > __GmLeft.uiPulse)
                        {
                            goto End;
                        }
                    }
                }
            }
            else
            {
                if (ucIRCheck[3] > GusDistance_R_Far)
                { /*  右边有墙时开始允许检测右边  */
                    cR = 1;
                }
            }
        }
    }
    /*
     *  设定运行任务，让电脑鼠走到支路的中心位置
     */
End:; // GuiSpeedCtr= 3;
      // __GmSPID.sRef=23;
}

void mouseStop(void)
{
    __GmSPID.sRef = 0;
    __GmWPID.sRef = 0;
    GuiSpeedCtr = 5;
}
/****************************************************************************************************
** Function name:       mouseTurnright
** Descriptions:        右转
** input parameters:    无
** output parameters:   无
** Returned value:      无         按步数转弯
*********************************************************************************************************/
void mouseTurnright(void)
{

    GW = 0;
    time = 0;
    __GucMouseState = __TURNRIGHT;
    __GmRight.cState = __MOTORRUN; //得加电脑鼠状态
    __GmLeft.cState = __MOTORRUN;
    GucMouseDir = (GucMouseDir + 1) % 4; /*  方向标记                    */

    __GmSPID.sRef = 80;
    __GmWPID.sRef = -70;
    while (1)
    {
        W = (float)GW * 5121 / 4096 / 2;
        if (W > 75000) //change from 51500 in 20160929
        {
            break;
        }
    }

    __GmWPID.sRef = 0;

    GuiSpeedCtr = 3;
    __GmLeft.uiPulse = 14000; //1200
    __GmLeft.uiPulseCtr = 0;
    __GmRight.uiPulse = 14000;
    __GmRight.uiPulseCtr = 0;
    while ((__GmRight.uiPulseCtr + 200) <= __GmRight.uiPulse)
        ;
    while ((__GmLeft.uiPulseCtr + 200) <= __GmLeft.uiPulse)
        ;
    __GucMouseState = __TURNRIGHT;
    GuiSpeedCtr = __SPEEDUP;
    __GmRight.cState = __MOTORSTOP;
    __GmLeft.cState = __MOTORSTOP;
    __GmRight.sSpeed = 0;
    __rightMotorContr();
    __GmLeft.sSpeed = 0;
    __leftMotorContr();
    __GmRight.uiPulseCtr = 0;
    __GmLeft.uiPulseCtr = 0;
}
/*********************************************************************************************************
** Function name:       mouseTurnleft
** Descriptions:        左转
** input parameters:    无
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void mouseTurnleft(void)
{
    time = 0;
    __GucMouseState = __TURNLEFT;
    __GmRight.cState = __MOTORRUN; //得加电脑鼠状态
    __GmLeft.cState = __MOTORRUN;
    GucMouseDir = (GucMouseDir + 3) % 4; /*  方向标记                    */

    __GmSPID.sRef = 80;
    __GmWPID.sRef = 70; //70
    GW = 0;
    while (1)
    {
        W = (float)GW * 5121 / 4096 / 2;
        if (W > 75000) //change from 53000 in 20160929
        {
            break;
        }
    }
    __GmWPID.sRef = 0;
    __GucMouseState = __GOAHEAD;

    GuiSpeedCtr = 3;
    __GmLeft.uiPulse = 14000;
    __GmLeft.uiPulseCtr = 0;
    __GmRight.uiPulse = 14000;
    __GmRight.uiPulseCtr = 0;
    while ((__GmRight.uiPulseCtr + 200) <= __GmRight.uiPulse)
        ;
    while ((__GmLeft.uiPulseCtr + 200) <= __GmLeft.uiPulse)
        ;

    __GucMouseState = __TURNLEFT;
    GuiSpeedCtr = __SPEEDUP;
    __GmRight.cState = __MOTORSTOP;
    __GmLeft.cState = __MOTORSTOP;
    __GmRight.sSpeed = 0;
    __rightMotorContr();
    __GmLeft.sSpeed = 0;
    __leftMotorContr();
    __GmRight.uiPulseCtr = 0;
    __GmLeft.uiPulseCtr = 0;
}

void mouseTurnrighthui(void)
{

    GW = 0;
    time = 0;
    __GucMouseState = __TURNRIGHT;
    __GmRight.cState = __MOTORRUN; //得加电脑鼠状态
    __GmLeft.cState = __MOTORRUN;
    GucMouseDir = (GucMouseDir + 1) % 4; /*  方向标记                    */
    while (1)
    {
        if (time > 210)
            break;
    }

    __GmWPID.sRef = 0;
    __GucMouseState = __GOAHEAD;
    GuiSpeedCtr = 3;
    __GmLeft.uiPulse = 8000; //1200
    __GmLeft.uiPulseCtr = 0;
    __GmRight.uiPulse = 8000;
    __GmRight.uiPulseCtr = 0;
    while ((__GmRight.uiPulseCtr + 200) <= __GmRight.uiPulse)
        ;
    while ((__GmLeft.uiPulseCtr + 200) <= __GmLeft.uiPulse)
        ;
    __GucMouseState = __TURNRIGHT;
    GuiSpeedCtr = __SPEEDUP;
    __GmRight.cState = __MOTORSTOP;
    __GmLeft.cState = __MOTORSTOP;
    __GmRight.sSpeed = 0;
    __rightMotorContr();
    __GmLeft.sSpeed = 0;
    __leftMotorContr();
    __GmRight.uiPulseCtr = 0;
    __GmLeft.uiPulseCtr = 0;
}

void mouseTurnlefthui(void)
{

    GW = 0;
    time = 0;
    __GucMouseState = __TURNLEFT;
    __GmRight.cState = __MOTORRUN; //得加电脑鼠状态
    __GmLeft.cState = __MOTORRUN;
    GucMouseDir = (GucMouseDir + 3) % 4; /*  方向标记                    */
    while (1)
    {
        if (time > 210)
            break;
    }

    __GucMouseState = __GOAHEAD;
    GuiSpeedCtr = 3;
    __GmLeft.uiPulse = 8000;
    __GmLeft.uiPulseCtr = 0;
    __GmRight.uiPulse = 8000;
    __GmRight.uiPulseCtr = 0;
    while ((__GmRight.uiPulseCtr + 200) <= __GmRight.uiPulse)
        ;
    while ((__GmLeft.uiPulseCtr + 200) <= __GmLeft.uiPulse)
        ;

    __GucMouseState = __TURNLEFT;
    GuiSpeedCtr = __SPEEDUP;
    __GmRight.cState = __MOTORSTOP;
    __GmLeft.cState = __MOTORSTOP;
    __GmRight.sSpeed = 0;
    __rightMotorContr();
    __GmLeft.sSpeed = 0;
    __leftMotorContr();
    __GmRight.uiPulseCtr = 0;
    __GmLeft.uiPulseCtr = 0;
}

/*********************************************************************************************************
** Function name:       mouseTurnleft
** Descriptions:        左转
** input parameters:    无
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void mouseTurnright_C(void)
{

    while ((__GmRight.uiPulseCtr + 200) <= __GmRight.uiPulse)
        ;
    while ((__GmLeft.uiPulseCtr + 200) <= __GmLeft.uiPulse)
        ;
    __GmLeft.uiPulse = 600; //change from 2600 in 20160929
    __GmLeft.uiPulseCtr = 0;
    __GmRight.uiPulse = 600; //change from 2600 in 20160929
    __GmRight.uiPulseCtr = 0;
    __GmWPID.sRef = -20;
    GW = 0;
    __GucMouseState = __TURNRIGHT;
    __GmRight.cState = __MOTORRUN; //得加电脑鼠状态
    __GmLeft.cState = __MOTORRUN;
    GucMouseDir = (GucMouseDir + 1) % 4; /*  方向标记                    */

    while (1)
    {
        if (GW > 42000)
        {
            break;
        }
    }
    __GmWPID.sRef = 0;
    __GucMouseState = __GOAHEAD;
    GuiSpeedCtr = 3;
    __GmLeft.uiPulse = 1200 - GuiTpusle_S;
    __GmLeft.uiPulseCtr = 0;
    __GmRight.uiPulse = 1200 - GuiTpusle_S;
    __GmRight.uiPulseCtr = 0;
    while ((__GmRight.uiPulseCtr + 200) <= __GmRight.uiPulse)
        ;
    while ((__GmLeft.uiPulseCtr + 200) <= __GmLeft.uiPulse)
        ;

    __GucMouseState = __TURNRIGHT;
    GuiSpeedCtr = __SPEEDUP;
    __GmRight.cState = __MOTORSTOP;
    __GmLeft.cState = __MOTORSTOP;
    __GmRight.uiPulseCtr = 0;
    __GmLeft.uiPulseCtr = 0;
}

/*********************************************************************************************************
** Function name:       mouseTurnleft
** Descriptions:        左转
** input parameters:    无
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void mouseTurnleft_C(void)
{

    while ((__GmRight.uiPulseCtr + 200) <= __GmRight.uiPulse)
        ;
    while ((__GmLeft.uiPulseCtr + 200) <= __GmLeft.uiPulse)
        ;

    __GmLeft.uiPulse = 3000;
    __GmLeft.uiPulseCtr = 0;
    __GmRight.uiPulse = 3000;
    __GmRight.uiPulseCtr = 0;

    __GmWPID.sRef = 20;
    GW = 0;
    __GucMouseState = __TURNLEFT;
    __GmRight.cState = __MOTORRUN; //得加电脑鼠状态
    __GmLeft.cState = __MOTORRUN;
    GucMouseDir = (GucMouseDir + 3) % 4; /*  方向标记                    */
    while (1)
    {
        if (GW > 40000)
        {
            break;
        }
    }
    __GmWPID.sRef = 0;
    __GucMouseState = __GOAHEAD;
    GuiSpeedCtr = 3;

    __GmLeft.uiPulse = 1200 - GuiTpusle_S;
    __GmLeft.uiPulseCtr = 0;
    __GmRight.uiPulse = 1200 - GuiTpusle_S;
    __GmRight.uiPulseCtr = 0;
    while ((__GmRight.uiPulseCtr + 200) <= __GmRight.uiPulse)
        ;
    while ((__GmLeft.uiPulseCtr + 200) <= __GmLeft.uiPulse)
        ;

    __GucMouseState = __TURNLEFT;
    GuiSpeedCtr = __SPEEDUP;
    __GmRight.cState = __MOTORSTOP;
    __GmLeft.cState = __MOTORSTOP;
    __GmRight.uiPulseCtr = 0;
    __GmLeft.uiPulseCtr = 0;
}
void mouseTurnright_Y1(void)
{
    __GmLeft.uiPulse = 6000;
    __GmLeft.uiPulseCtr = 0;
    __GmRight.uiPulse = 6000;
    __GmRight.uiPulseCtr = 0;
    __GmSPID.sRef = 0;
    __GmWPID.sRef = -20;
    GW = 0;
    __GucMouseState = __TURNRIGHTY;
    __GmRight.cState = __MOTORRUN; //得加电脑鼠状态
    __GmLeft.cState = __MOTORRUN;
    GucMouseDir = (GucMouseDir + 1) % 4; /*  方向标记                    */

    while (1)
    {
        W = (float)GW * 5121 / 4096 / 2;
        if (W > 90000)
        {
            break;
        }
    }
    __mazeInfDebug();

    __GmWPID.sRef = 0;
    __GmSPID.sRef = 50;

    GuiSpeedCtr = __SPEEDUP;
    __GmRight.cState = __MOTORSTOP;
    __GmLeft.cState = __MOTORSTOP;
    __GmRight.sSpeed = 0;
    __rightMotorContr();
    __GmLeft.sSpeed = 0;
    __leftMotorContr();
    __GmRight.uiPulseCtr = 0;
    __GmLeft.uiPulseCtr = 0;
}

/*********************************************************************************************************
** Function name:       mouseTurnleft
** Descriptions:        左转
** input parameters:    无
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void mouseTurnleft_Y1(void)
{
    __GmLeft.uiPulse = 6000;
    __GmLeft.uiPulseCtr = 0;
    __GmRight.uiPulse = 6000;
    __GmRight.uiPulseCtr = 0;

    __GmWPID.sRef = 24;
    GW = 0;
    __GucMouseState = __TURNLEFTY;

    __GmRight.cState = __MOTORRUN; //得加电脑鼠状态
    __GmLeft.cState = __MOTORRUN;
    GucMouseDir = (GucMouseDir + 3) % 4; /*  方向标记                    */
    while (1)
    {
        W = (float)GW * 5121 / 4096 / 2;
        if (W > 90000)
        {
            break;
        }
    }
    __mazeInfDebug();
    __GmWPID.sRef = 0;
    __GmSPID.sRef = 25;

    GuiSpeedCtr = __SPEEDUP;
    __GmRight.cState = __MOTORSTOP;
    __GmLeft.cState = __MOTORSTOP;
    __GmRight.sSpeed = 0;
    __rightMotorContr();
    __GmLeft.sSpeed = 0;
    __leftMotorContr();
    __GmRight.uiPulseCtr = 0;
    __GmLeft.uiPulseCtr = 0;
}

void mouseTurnright_Y(void)
{

    GW = 0;
    time = 0;
    __GucMouseState = __TURNRIGHT;
    __GmRight.cState = __MOTORRUN; //得加电脑鼠状态
    __GmLeft.cState = __MOTORRUN;
    GucMouseDir = (GucMouseDir + 1) % 4; /*  方向标记                    */
    while (1)
    {
        if (time > 210)
            break;
    }

    __GmWPID.sRef = 0;
    __GucMouseState = __GOAHEAD;
    GuiSpeedCtr = 3;
    __GmLeft.uiPulse = 8000; //1200
    __GmLeft.uiPulseCtr = 0;
    __GmRight.uiPulse = 8000;
    __GmRight.uiPulseCtr = 0;
    while ((__GmRight.uiPulseCtr + 200) <= __GmRight.uiPulse)
        ;
    while ((__GmLeft.uiPulseCtr + 200) <= __GmLeft.uiPulse)
        ;
    __GucMouseState = __TURNRIGHT;
    GuiSpeedCtr = __SPEEDUP;
    __GmRight.cState = __MOTORSTOP;
    __GmLeft.cState = __MOTORSTOP;
    __GmRight.sSpeed = 0;
    __rightMotorContr();
    __GmLeft.sSpeed = 0;
    __leftMotorContr();
    __GmRight.uiPulseCtr = 0;
    __GmLeft.uiPulseCtr = 0;
}
/*********************************************************************************************************
** Function name:       mouseTurnleft
** Descriptions:        左转
** input parameters:    无
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void mouseTurnleft_Y(void)
{

    GW = 0;
    time = 0;
    __GucMouseState = __TURNLEFT;
    __GmRight.cState = __MOTORRUN; //得加电脑鼠状态
    __GmLeft.cState = __MOTORRUN;
    GucMouseDir = (GucMouseDir + 3) % 4; /*  方向标记                    */
    while (1)
    {
        if (time > 210)
            break;
    }

    __GucMouseState = __GOAHEAD;

    GuiSpeedCtr = 3;
    __GmLeft.uiPulse = 8000;
    __GmLeft.uiPulseCtr = 0;
    __GmRight.uiPulse = 8000;
    __GmRight.uiPulseCtr = 0;
    while ((__GmRight.uiPulseCtr + 200) <= __GmRight.uiPulse)
        ;
    while ((__GmLeft.uiPulseCtr + 200) <= __GmLeft.uiPulse)
        ;

    __GucMouseState = __TURNLEFT;
    GuiSpeedCtr = __SPEEDUP;
    __GmRight.cState = __MOTORSTOP;
    __GmLeft.cState = __MOTORSTOP;
    __GmRight.sSpeed = 0;
    __rightMotorContr();
    __GmLeft.sSpeed = 0;
    __leftMotorContr();
    __GmRight.uiPulseCtr = 0;
    __GmLeft.uiPulseCtr = 0;
}

void onestep(void)
{
    __GmRight.uiPulse = 22000;
    __GmRight.uiPulseCtr = 0;
    __GmLeft.uiPulse = 22000;
    __GmLeft.uiPulseCtr = 0;
    while ((__GmRight.uiPulseCtr + 200) <= __GmRight.uiPulse)
        ;
    while ((__GmLeft.uiPulseCtr + 200) <= __GmLeft.uiPulse)
        ;
}

void onestep1(void)
{
    __GmRight.uiPulse = 12000;
    __GmRight.uiPulseCtr = 0;
    __GmLeft.uiPulse = 12000;
    __GmLeft.uiPulseCtr = 0;
    while ((__GmRight.uiPulseCtr + 200) <= __GmRight.uiPulse)
        ;
    while ((__GmLeft.uiPulseCtr + 200) <= __GmLeft.uiPulse)
        ;
}
/*********************************************************************************************************
** Function name:       mouseTurnback
** Descriptions:        根据前方近距，旋转180度
** input parameters:    无
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void mouseTurnback1(void)
{
    __GmSPID.sRef = 0;
    GW = 0;
    time = 0;
    __GucMouseState = __TURNBACK;
    __GmLeft.cState = __MOTORRUN;
    __GmRight.cState = __MOTORRUN;
    GucMouseDir = (GucMouseDir + 2) % 4;
    while (1)
    {
        if (time > 620)
            break;
    }

    __GucMouseState = __GOAHEAD;
    mouseStop();
    __delay(1500000);
    GuiSpeedCtr = __SPEEDUP;
    __GmRight.cState = __MOTORSTOP;
    __GmLeft.cState = __MOTORSTOP;
    __GmRight.sSpeed = 0;
    __rightMotorContr();
    __GmLeft.sSpeed = 0;
    __leftMotorContr();
    __GmRight.uiPulseCtr = 0;
    __GmLeft.uiPulseCtr = 0;
}

void mouseTurnback(void)
{
    if (GucFrontNear)
    {
        __GmSPID.sRef = 120;
        while ((ucIRCheck[0] < GusDistance_FL_Near) || (ucIRCheck[1] < GusDistance_FR_Near))
            ;
        __GmRight.uiPulse = 1000;
        __GmRight.uiPulseCtr = 0;
        __GmLeft.uiPulse = 1000;
        __GmLeft.uiPulseCtr = 0;
        while ((__GmRight.uiPulseCtr + 200) <= __GmRight.uiPulse)
            ;
        while ((__GmLeft.uiPulseCtr + 200) <= __GmLeft.uiPulse)
            ;
    }
    GucFrontNear = 0;
    backflag = 1;
    __GmSPID.sRef = 0;
    mouseStop();
    GW = 0;
    time = 0;
    __GucMouseState = __TURNBACK;
    __GmLeft.cState = __MOTORRUN;
    __GmRight.cState = __MOTORRUN;
    GucMouseDir = (GucMouseDir + 2) % 4;
    __GmWPID.sRef = 50;

    while (1)
    {
        if (GW > 280000) //change from 180000 in 20160929
        {
            break;
        }
    }
    __GmWPID.sRef = 0;
    __GucMouseState = __GOBACK;
    __GmSPID.sRef = -40;
    __GmRight.uiPulse = 7000;
    __GmRight.uiPulseCtr = 0;
    __GmLeft.uiPulse = 7000;
    __GmLeft.uiPulseCtr = 0;
    while ((__GmRight.uiPulseCtr + 200) <= __GmRight.uiPulse)
        ;
    while ((__GmLeft.uiPulseCtr + 200) <= __GmLeft.uiPulse)
        ;
    mouseStop();
    __GucMouseState = __GOAHEAD;
    __delay(5000000);

    GuiSpeedCtr = __SPEEDUP;
    __GmSPID.sRef = 120;

    __GmRight.uiPulse = 25000;
    __GmRight.uiPulseCtr = 0;
    __GmLeft.uiPulse = 25000;
    __GmLeft.uiPulseCtr = 0;
    while ((__GmRight.uiPulseCtr + 200) <= __GmRight.uiPulse)
        ;
    while ((__GmLeft.uiPulseCtr + 200) <= __GmLeft.uiPulse)
        ;
    __GmRight.cState = __MOTORSTOP;
    __GmLeft.cState = __MOTORSTOP;

    __GmRight.sSpeed = 0;
    __rightMotorContr();
    __GmLeft.sSpeed = 0;
    __leftMotorContr();
    __GmRight.uiPulseCtr = 30000;
    __GmLeft.uiPulseCtr = 30000;
}

void mouseTurnbackqi(void)
{
    if (GucFrontNear)
    {
        __GmSPID.sRef = 120;
        while ((ucIRCheck[0] < GusDistance_FL_Near) || (ucIRCheck[1] < GusDistance_FR_Near))
            ;
        __GmRight.uiPulse = 1000;
        __GmRight.uiPulseCtr = 0;
        __GmLeft.uiPulse = 1000;
        __GmLeft.uiPulseCtr = 0;
        while ((__GmRight.uiPulseCtr + 200) <= __GmRight.uiPulse)
            ;
        while ((__GmLeft.uiPulseCtr + 200) <= __GmLeft.uiPulse)
            ;
    }
    GucFrontNear = 0;
    backflag = 1;
    __GmSPID.sRef = 0;
    mouseStop();
    GW = 0;
    time = 0;
    __GucMouseState = __TURNBACK;
    __GmLeft.cState = __MOTORRUN;
    __GmRight.cState = __MOTORRUN;
    GucMouseDir = (GucMouseDir + 2) % 4;
    __GmWPID.sRef = 50;

    while (1)
    {
        if (GW > 320000)
        {
            break;
        }
    }
    __GmWPID.sRef = 0;

    mouseStop();
    __GucMouseState = __GOAHEAD;
    __delay(2000000);

    GuiSpeedCtr = __SPEEDUP;
    __GmSPID.sRef = 120;

    __GmRight.cState = __MOTORSTOP;
    __GmLeft.cState = __MOTORSTOP;

    __GmRight.sSpeed = 0;
    __rightMotorContr();
    __GmLeft.sSpeed = 0;
    __leftMotorContr();
    __GmRight.uiPulseCtr = 30000;
    __GmLeft.uiPulseCtr = 30000;
}

/*********************************************************************************************************
** Function name:       mouseTurnback_Y
** Descriptions:        原地旋转180度
** input parameters:    无
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void mouseTurnback_Y1(void)
{
    __GmSPID.sRef = 0;
    GW = 0;
    time = 0;
    __GucMouseState = __TURNBACK;
    __GmLeft.cState = __MOTORRUN;
    __GmRight.cState = __MOTORRUN;
    GucMouseDir = (GucMouseDir + 2) % 4;
    while (1)
    {
        if (time > 620)
            break;
    }

    __GucMouseState = __GOAHEAD;
    mouseStop();

    __delay(5000000);
    __GmSPID.sRef = 120;
    __mouseCoorUpdate();
    GuiSpeedCtr = __SPEEDUP;
    __GmRight.sSpeed = 0;
    __rightMotorContr();
    __GmLeft.sSpeed = 0;
    __leftMotorContr();
    __GmRight.uiPulseCtr = 0;
    __GmLeft.uiPulseCtr = 0;
    backflag = 1;
    backflag2 = 1;
}

void mouseTurnback_Y(void)
{

    __GmSPID.sRef = 0;
    mouseStop();
    __delay(1500000);

    GW = 0;
    time = 0;
    __GucMouseState = __TURNBACK;
    __GmLeft.cState = __MOTORRUN;
    __GmRight.cState = __MOTORRUN;
    GucMouseDir = (GucMouseDir + 2) % 4;
    while (1)
    {
        if (time > 597)
            break;
    }
    __GucMouseState = __GOAHEAD;
    mouseStop();
    __delay(1500000);

    __GmWPID.sRef = 0;
    GuiSpeedCtr = __SPEEDUP;
    __GmRight.cState = __MOTORSTOP;
    __GmLeft.cState = __MOTORSTOP;
    __GmRight.sSpeed = 0;
    __rightMotorContr();
    __GmLeft.sSpeed = 0;
    __leftMotorContr();
    __GmRight.uiPulseCtr = 0;
    __GmLeft.uiPulseCtr = 0;
}

/*********************************************************************************************************
** Function name:       mazeSearch
** Descriptions:        前进N格
** input parameters:    iNblock: 前进的格数
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void mazeSearch(void) //搜索连续转弯
{
    int8 cL = 0, cR = 0, cCoor = 1;
    // GusDistance_FL_Near = 80;
    //GusDistance_FL_Near = 80;
    if (__GmLeft.cState)
    {
        cCoor = 0;
    }
    if ((__GucMouseState == __TURNRIGHT) || (__GucMouseState == __TURNLEFT))
    {
        __GmLeft.uiPulseCtr = 30000;
        __GmRight.uiPulseCtr = 30000;
        cL = 1;
        cR = 1;
        if (((__GucDistance[__FRONTR] != 0) && (__GucDistance[__FRONTL] != 0)) || ((__GucDistance[__LEFT] & 0x01) == 0) || ((__GucDistance[__RIGHT] & 0x01) == 0))
        {
            if ((__GucDistance[__FRONTR] != 0) && (__GucDistance[__FRONTL] != 0))
                GuiTpusle_LR = 15000;
            else
                GuiTpusle_LR = 15000;
        }
        else
        {
            GuiTpusle_LR = 0;
        }
    }
    else
    {
        GuiTpusle_LR = 0;
    }
    __GucMouseState = __GOAHEAD;
    __GiMaxSpeed = SEARCHSPEED;
    __GmRight.uiPulse = MAZETYPE * ONEBLOCK;
    __GmLeft.uiPulse = MAZETYPE * ONEBLOCK;
    __GmRight.cState = __MOTORRUN;
    __GmLeft.cState = __MOTORRUN;
    GuiSpeedCtr = __SPEEDUP;
    while (__GmLeft.cState != __MOTORSTOP)
    {

        if (__GmLeft.uiPulseCtr >= ONEBLOCK)
        { /*  判断是否走完一格*/
            __GmLeft.uiPulse -= ONEBLOCK;
            __GmLeft.uiPulseCtr -= ONEBLOCK;
            if (cCoor)
            {
                if (((__GucDistance[__FRONTR] != 0) && (__GucDistance[__FRONTL] != 0)) && (ucIRCheck[2] > GusDistance_L_Far) && (ucIRCheck[3] > GusDistance_R_Far)) //0x01
                {
                    GucFrontNear = 1;

                    goto End;
                }
                __mouseCoorUpdate(); /*  更新坐标                    */
            }
            else
            {
                cCoor = 1;
            }
        }
        if (__GmRight.uiPulseCtr >= ONEBLOCK)
        { /*  判断是否走完一格            */
            __GmRight.uiPulse -= ONEBLOCK;
            __GmRight.uiPulseCtr -= ONEBLOCK;
        }

        if (cL)
        { /*  是否允许检测左边            */
            if ((__GucDistance[__LEFT] & 0x01) == 0)
            { /*  左边有支路，跳出程序        */

                __GmRight.uiPulse = __GmRight.uiPulseCtr + 21000 - GuiTpusle_LR; //10800 //7500//5000    //1500
                __GmLeft.uiPulse = __GmLeft.uiPulseCtr + 21000 - GuiTpusle_LR;
                while ((__GucDistance[__LEFT] & 0x01) == 0)
                {

                    if ((__GmLeft.uiPulseCtr + 100) > __GmLeft.uiPulse)
                    {

                        goto End;
                    }
                }
                __GmRight.uiPulse = MAZETYPE * ONEBLOCK;
                __GmLeft.uiPulse = MAZETYPE * ONEBLOCK;
                GuiSpeedCtr = __SPEEDUP;
            }
        }
        else
        { /*  左边有墙时开始允许检测左边  */
            if (ucIRCheck[2] > GusDistance_L_Far)
            {
                cL = 1;
            }
        }
        if (cR)
        { /*  是否允许检测右边            */
            if ((__GucDistance[__RIGHT] & 0x01) == 0)
            { /*  右边有支路，跳出程序        */

                __GmRight.uiPulse = __GmRight.uiPulseCtr + 21000 - GuiTpusle_LR; //6500 //5000   //3300
                __GmLeft.uiPulse = __GmLeft.uiPulseCtr + 21000 - GuiTpusle_LR;
                while ((__GucDistance[__RIGHT] & 0x01) == 0)
                {

                    if ((__GmLeft.uiPulseCtr + 100) > __GmLeft.uiPulse)
                    {
                        goto End;
                    }
                }
                __GmRight.uiPulse = MAZETYPE * ONEBLOCK;
                __GmLeft.uiPulse = MAZETYPE * ONEBLOCK;
                GuiSpeedCtr = __SPEEDUP;
            }
        }
        else
        {
            if (ucIRCheck[3] > GusDistance_R_Far)
            {
                cR = 1;
                //  flag=1;
            }
        }
    }
End:

    __mouseCoorUpdate(); /*  更新坐标                    */
}

/*********************************************************************************************************
** Function name:       __mouseCoorUpdate
** Descriptions:        根据当前方向更新坐标值
** input parameters:    无
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void __mouseCoorUpdate(void)
{

    switch (GucMouseDir)
    {

    case 0:
        GmcMouse.cY++;
        break;

    case 1:
        GmcMouse.cX++;
        break;

    case 2:
        GmcMouse.cY--;
        break;

    case 3:
        GmcMouse.cX--;
        break;

    default:
        break;
    }

    __mazeInfDebug();
    __wallCheck();
}

void __mazeInfDebug(void)
{
    /*
     *  显示方向
     */
    switch (GucMouseDir)
    {

    case 0:
        zlg7289Download(2, 3, 0, 0x47); /*  向前，用F表示               */
        break;

    case 1:
        zlg7289Download(2, 3, 0, 0x77); /*  向右，用R表示               */
        break;

    case 2:
        zlg7289Download(2, 3, 0, 0x1f); /*  向后，用b表示               */
        break;

    case 3:
        zlg7289Download(2, 3, 0, 0x0e); /*  向左，用L表示               */
        break;

    default:
        zlg7289Download(2, 3, 0, 0x4f); /*  错误，用E表示               */
        break;
    }
    /*
     *  显示坐标
     */
    zlg7289Download(1, 0, 0, GmcMouse.cX / 10);
    zlg7289Download(1, 1, 0, GmcMouse.cX % 10);
    zlg7289Download(1, 6, 0, GmcMouse.cY / 10);
    zlg7289Download(1, 7, 0, GmcMouse.cY % 10);
}

/*********************************************************************************************************
** Function name:       __wallCheck
** Descriptions:        根据传感器检测结果判断是否存在墙壁
** input parameters:    无
** output parameters:   无
** Returned value:      cValue: 低三位从左到右一次代表左前右。1为有墙，0为没墙。
*********************************************************************************************************/
void __wallCheck(void)
{
    uint8 ucMap = 0;
    uint8 uctemp = 0;
    ucMap |= MOUSEWAY_B;

    if (ucIRCheck[2] > GusDistance_L_Far) //左边有挡板，但是距离较远
    {
        ucMap &= ~MOUSEWAY_L; //相对方向的左边有墙，并却转换成绝对坐标
        uctemp |= 0x06;
    }
    else
    {
        ucMap |= MOUSEWAY_L; //相对方向的左边无墙，并却转换成绝对坐标
    }

    if ((ucIRCheck[0] > GusDistance_FL_Far) && (ucIRCheck[1] > GusDistance_FR_Far))
    {
        ucMap &= ~MOUSEWAY_F;
        uctemp |= 0x40;
    }
    else
    {
        ucMap |= MOUSEWAY_F;
    }
    if (ucIRCheck[3] > GusDistance_R_Far)
    {
        ucMap &= ~MOUSEWAY_R;
        uctemp |= 0x30;
    }
    else
    {
        ucMap |= MOUSEWAY_R;
    }
    GucMapBlock0[GmcMouse.cX][GmcMouse.cY] = ucMap;
    GucMapBlock[GmcMouse.cX][GmcMouse.cY] = ucMap;
    GucMapBlock1[GmcMouse.cX][GmcMouse.cY] = ucMap;
    if (GmcMouse.cY < (MAZETYPE - 1))
    {
        GucMapBlock1[GmcMouse.cX][GmcMouse.cY + 1] &= ~(((~ucMap) & 0x01) * 4);
    } /*将该坐标周围坐标墙壁资料更改  注：洪水用*/
    if (GmcMouse.cX < (MAZETYPE - 1))
    {
        GucMapBlock1[GmcMouse.cX + 1][GmcMouse.cY] &= ~(((~ucMap) & 0x02) * 4);
    }
    if (GmcMouse.cY > 0)
    {
        GucMapBlock1[GmcMouse.cX][GmcMouse.cY - 1] &= ~(((~ucMap) & 0x04) / 4);
    }
    if (GmcMouse.cX > 0)
    {
        GucMapBlock1[GmcMouse.cX - 1][GmcMouse.cY] &= ~(((~ucMap) & 0x08) / 4);
    }

    if (GmcMouse.cY < (MAZETYPE - 1))
    {
        GucMapBlock[GmcMouse.cX][GmcMouse.cY + 1] |= ((ucMap & 0x01) * 4);
    } /*将该坐标周围坐标墙壁资料更改  注：在初始为有墙时管用*/
    if (GmcMouse.cX < (MAZETYPE - 1))
    {
        GucMapBlock[GmcMouse.cX + 1][GmcMouse.cY] |= ((ucMap & 0x02) * 4);
    }
    if (GmcMouse.cY > 0)
    {
        GucMapBlock[GmcMouse.cX][GmcMouse.cY - 1] |= ((ucMap & 0x04) / 4);
    }
    if (GmcMouse.cX > 0)
    {
        GucMapBlock[GmcMouse.cX - 1][GmcMouse.cY] |= ((ucMap & 0x08) / 4);
    }

    zlg7289Download(2, 2, 0, uctemp);
}

uint8 DenggaoCheck(void) //搜索中制作等高图stop，原地转弯判断
{
    if ((__GucDistance[__FRONT]) && (__GucDistance[__LEFT] & 0x01) && (__GucDistance[__RIGHT] & 0x01))
    {
        return (true);
    }
    else
    {
        return (false);
    }
}
uint8 PulseCtrCheck(void) ////搜索中制作等高图stop，已走步数判断
{
    if (__GmLeft.uiPulseCtr > (__GmLeft.uiPulse + 4000))
    {
        return (true);
    }
    else
    {
        return (false);
    }
}

void EXTI3_IRQHandler(void)
{
    char key;
    if (EXTI_GetITStatus(EXTI_Line3) != RESET)
    {
        key = zlg7289Key();
        if (key != 0xff)
        {
            if (key >= 4)
            {
                if (key == 10)
                {
                    DIS[0] = GusDistance_L_Near;
                    DIS[1] = GusDistance_R_Near;
                    DIS[2] = GusDistance_L_Mid;
                    DIS[3] = GusDistance_R_Mid;
                    DIS[4] = GusDistance_L_Far;
                    DIS[5] = GusDistance_R_Far;
                    DIS[6] = GusDistance_FL_Far;
                    DIS[7] = GusDistance_FR_Far;
                    DIS[8] = GusDistance_FL_Near;
                    DIS[9] = GusDistance_FR_Near;
                    STMFLASH_Write(IR_SAVE_ADDRESS, (u16 *)DIS, 10);
                    __delay(1000000);
                }

                else
                {
                    Tab = key;
                    ir_flag = 1;
                }
            }
            switch (Tab)
            {
            case 4:
                if (ir_flag == 1)
                {
                    GusDistance_L_Near = ucIRCheck[2];
                    GusDistance_R_Near = ucIRCheck[3];
                    ir_flag = 0;
                }
                if (key == 1)
                {
                    GusDistance_L_Near += 10;
                    zlg7289Download(0, 4, 0, GusDistance_L_Near / 1000);
                    zlg7289Download(0, 5, 0, GusDistance_L_Near / 100 % 10);
                    zlg7289Download(0, 6, 0, GusDistance_L_Near / 10 % 10);
                    zlg7289Download(0, 7, 0, GusDistance_L_Near % 10);
                }
                else if (key == 3)
                {
                    GusDistance_L_Near -= 2;
                    zlg7289Download(0, 5, 0, GusDistance_L_Near / 100);
                    zlg7289Download(0, 6, 0, GusDistance_L_Near / 10 % 10);
                    zlg7289Download(0, 7, 0, GusDistance_L_Near % 10);
                }
                else if (key == 0)
                {
                    GusDistance_R_Near += 10;
                    zlg7289Download(0, 5, 0, GusDistance_R_Near / 100);
                    zlg7289Download(0, 6, 0, GusDistance_R_Near / 10 % 10);
                    zlg7289Download(0, 7, 0, GusDistance_R_Near % 10);
                }
                else if (key == 2)
                {
                    GusDistance_R_Near -= 2;
                    zlg7289Download(0, 5, 0, GusDistance_R_Near / 100);
                    zlg7289Download(0, 6, 0, GusDistance_R_Near / 10 % 10);
                    zlg7289Download(0, 7, 0, GusDistance_R_Near % 10);
                }
                break;
            case 5:
                if (ir_flag == 1)
                {
                    GusDistance_L_Mid = ucIRCheck[2];
                    GusDistance_R_Mid = ucIRCheck[3];
                    ir_flag = 0;
                }
                if (key == 1)
                {
                    GusDistance_L_Mid += 10;
                    zlg7289Download(0, 5, 0, GusDistance_L_Mid / 100);
                    zlg7289Download(0, 6, 0, GusDistance_L_Mid / 10 % 10);
                    zlg7289Download(0, 7, 0, GusDistance_L_Mid % 10);
                }
                else if (key == 3)
                {
                    GusDistance_L_Mid -= 2;
                    zlg7289Download(0, 5, 0, GusDistance_L_Mid / 100);
                    zlg7289Download(0, 6, 0, GusDistance_L_Mid / 10 % 10);
                    zlg7289Download(0, 7, 0, GusDistance_L_Mid % 10);
                }
                else if (key == 0)
                {
                    GusDistance_R_Mid += 10;
                    zlg7289Download(0, 5, 0, GusDistance_R_Mid / 100);
                    zlg7289Download(0, 6, 0, GusDistance_R_Mid / 10 % 10);
                    zlg7289Download(0, 7, 0, GusDistance_R_Mid % 10);
                }
                else if (key == 2)
                {
                    GusDistance_R_Mid -= 2;
                    zlg7289Download(0, 5, 0, GusDistance_R_Mid / 100);
                    zlg7289Download(0, 6, 0, GusDistance_R_Mid / 10 % 10);
                    zlg7289Download(0, 7, 0, GusDistance_R_Mid % 10);
                }
                break;
            case 6:
                if (ir_flag == 1)
                {
                    GusDistance_L_Far = ucIRCheck[2];
                    GusDistance_R_Far = ucIRCheck[3];
                    ir_flag = 0;
                }
                if (key == 1)
                {
                    GusDistance_L_Far += 10;
                    zlg7289Download(0, 5, 0, GusDistance_L_Far / 100);
                    zlg7289Download(0, 6, 0, GusDistance_L_Far / 10 % 10);
                    zlg7289Download(0, 7, 0, GusDistance_L_Far % 10);
                }
                else if (key == 3)
                {
                    GusDistance_L_Far -= 2;
                    zlg7289Download(0, 5, 0, GusDistance_L_Far / 100);
                    zlg7289Download(0, 6, 0, GusDistance_L_Far / 10 % 10);
                    zlg7289Download(0, 7, 0, GusDistance_L_Far % 10);
                }
                else if (key == 0)
                {
                    GusDistance_R_Far += 10;
                    zlg7289Download(0, 5, 0, GusDistance_R_Far / 100);
                    zlg7289Download(0, 6, 0, GusDistance_R_Far / 10 % 10);
                    zlg7289Download(0, 7, 0, GusDistance_R_Far % 10);
                }
                else if (key == 2)
                {
                    GusDistance_R_Far -= 2;
                    zlg7289Download(0, 5, 0, GusDistance_R_Far / 100);
                    zlg7289Download(0, 6, 0, GusDistance_R_Far / 10 % 10);
                    zlg7289Download(0, 7, 0, GusDistance_R_Far % 10);
                }
                break;
            case 7:
                if (ir_flag == 1)
                {
                    GusDistance_FL_Far = ucIRCheck[0];
                    GusDistance_FR_Far = ucIRCheck[1];
                    ir_flag = 0;
                }
                if (key == 1)
                {
                    GusDistance_FL_Far += 10;
                    zlg7289Download(0, 5, 0, GusDistance_FL_Far / 100);
                    zlg7289Download(0, 6, 0, GusDistance_FL_Far / 10 % 10);
                    zlg7289Download(0, 7, 0, GusDistance_FL_Far % 10);
                }
                else if (key == 3)
                {
                    GusDistance_FL_Far -= 2;
                    zlg7289Download(0, 5, 0, GusDistance_FL_Far / 100);
                    zlg7289Download(0, 6, 0, GusDistance_FL_Far / 10 % 10);
                    zlg7289Download(0, 7, 0, GusDistance_FL_Far % 10);
                }
                else if (key == 0)
                {
                    GusDistance_FR_Far += 10;
                    zlg7289Download(0, 5, 0, GusDistance_FR_Far / 100);
                    zlg7289Download(0, 6, 0, GusDistance_FR_Far / 10 % 10);
                    zlg7289Download(0, 7, 0, GusDistance_FR_Far % 10);
                }
                else if (key == 2)
                {
                    GusDistance_FR_Far -= 2;
                    zlg7289Download(0, 5, 0, GusDistance_FR_Far / 100);
                    zlg7289Download(0, 6, 0, GusDistance_FR_Far / 10 % 10);
                    zlg7289Download(0, 7, 0, GusDistance_FR_Far % 10);
                }
                break;
            case 8:
                if (ir_flag == 1)
                {
                    GusDistance_FL_Near = ucIRCheck[0];
                    GusDistance_FR_Near = ucIRCheck[1];
                    ir_flag = 0;
                }
                if (key == 1)
                {
                    GusDistance_FL_Near += 10;
                    zlg7289Download(0, 5, 0, GusDistance_FL_Near / 100);
                    zlg7289Download(0, 6, 0, GusDistance_FL_Near / 10 % 10);
                    zlg7289Download(0, 7, 0, GusDistance_FL_Near % 10);
                }
                else if (key == 3)
                {
                    GusDistance_FL_Near -= 2;
                    zlg7289Download(0, 5, 0, GusDistance_FL_Near / 100);
                    zlg7289Download(0, 6, 0, GusDistance_FL_Near / 10 % 10);
                    zlg7289Download(0, 7, 0, GusDistance_FL_Near % 10);
                }
                else if (key == 0)
                {
                    GusDistance_FR_Near += 10;
                    zlg7289Download(0, 5, 0, GusDistance_FR_Near / 100);
                    zlg7289Download(0, 6, 0, GusDistance_FR_Near / 10 % 10);
                    zlg7289Download(0, 7, 0, GusDistance_FR_Near % 10);
                }
                else if (key == 2)
                {
                    GusDistance_FR_Near -= 2;
                    zlg7289Download(0, 5, 0, GusDistance_FR_Near / 100);
                    zlg7289Download(0, 6, 0, GusDistance_FR_Near / 10 % 10);
                    zlg7289Download(0, 7, 0, GusDistance_FR_Near % 10);
                }
                break;

            default:
                break;
            }
            __delay(200000);
            EXTI_ClearITPendingBit(EXTI_Line3);
        }
    }
}